package behavior;

import sensor.USensor;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;
import motor.carrito;

public class Dispersion implements Behavior{

	private USensor usensor;
    private carrito carro;
    private boolean stop;
    private boolean lost;
    private int timeout;
    
	public Dispersion(carrito carro, USensor usensor){
		this.carro = carro;
		this.usensor= usensor;
		this.stop = false;
		this.lost = false;
		this.timeout = 0;
	}
	public boolean takeControl() {
		// TODO Auto-generated method stub
		int val = usensor.getLastDistance();
		
		if(this.usensor.detectLostAgent() && carro.isAgent())
		{
			//.println("Perdi al lider");
			this.lost = true;
			return true;
		}
		else if(((this.usensor.detectLeaveAgent()) || (carro.isAgent())) && (val < 40)&& (val > 20))
		{
			//.println("Tome control de DISPERSION 1");
			this.stop = false;
			this.lost = false;
			return true;
				
		}
		else if((carro.isAgent()) && (val < 20) && (this.timeout < 25))
		{
			//.println("Tome control de DISPERSION 2");
			this.lost = false;
			this.stop = true;
			return true;
				
		}
		else if((usensor.detectComeAgent()) && (this.timeout < 25))
		{
			//.println("Tome control de DISPERSION 3 ");
			this.lost = false;
			this.stop = true;
			return true;
		}
		else
		{
			this.timeout = 0;
			return false;
		}
		
	}

	public void action() {
		// TODO Auto-generated method stub
		//carro.Stop();
		if(this.lost)
		{
			this.carro.setAgent(false);
			carro.SetMovingSpeed(250);
			carro.SpinRight();
			Delay.msDelay(1000);
			usensor.cleanDistances();
		}
		else
		{
			carro.setAgent(true);
			if(!this.stop)
			{
				
				carro.SetMovingSpeed(250);
				carro.Forward();
			}
			else
			{
				this.timeout++;
				carro.Stop();
			}
			this.stop = false;
		}
		
		//carro.setAgent(false);
		//carro.SpinLeft();
		//Delay.msDelay(3000);
		//carro.Stop();
	}

	public void suppress() {
	}

}
